(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    172423,       4829]
NotebookOptionsPosition[    168683,       4707]
NotebookOutlinePosition[    169141,       4725]
CellTagsIndexPosition[    169098,       4722]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "2", "d", " ", "kneed", " ", "biped", " ", "without", " ", "torso"}], 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"with", " ", "all", " ", "output", " ", "functions"}], "*)"}], 
  "\[IndentingNewLine]", "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}], "\n", 
   RowBox[{"<<", "RobotLinks.m"}], "\[IndentingNewLine]", 
   RowBox[{"<<", "Linearize.m"}], "\[IndentingNewLine]", 
   RowBox[{"<<", "ToMatlabMod.m"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"A_", "?", "MatrixQ"}], "]"}], ":=", 
     RowBox[{"ParallelTable", "[", 
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{"A", "\[LeftDoubleBracket]", 
         RowBox[{"i", ",", "j"}], "\[RightDoubleBracket]"}], "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", 
         RowBox[{
          RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "1", 
          "\[RightDoubleBracket]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"j", ",", 
         RowBox[{
          RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "2", 
          "\[RightDoubleBracket]"}]}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"A_", "?", "VectorQ"}], "]"}], ":=", 
     RowBox[{"ParallelTable", "[", 
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{"A", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", 
         RowBox[{"Length", "[", "A", "]"}]}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{"ParallelSimplify", "[", "A_", "]"}], ":=", 
     RowBox[{"Simplify", "[", "A", "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609372493992*^9}, {3.527609663926358*^9, 
   3.5276096847720747`*^9}, {3.527611654697356*^9, 3.5276117039558363`*^9}, {
   3.527683805470429*^9, 3.5276838110762777`*^9}, {3.5292676404146423`*^9, 
   3.529267644932723*^9}, {3.5293249527117367`*^9, 3.529324952933673*^9}, {
   3.5297806807408457`*^9, 3.5297806818902082`*^9}, {3.530976889113785*^9, 
   3.530976900871913*^9}, {3.530977237261526*^9, 3.53097724669871*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
     "]"}], ";"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"(*", 
    RowBox[{"choose", " ", "the", " ", "output", " ", "combination"}], "*)"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LineNumber", " ", "=", " ", "1"}], ";", "\n", 
    RowBox[{"IndexAllPre", " ", "=", " ", 
     RowBox[{"Import", "[", 
      RowBox[{"\"\<data/IndexAll.mat\>\"", ",", "\"\<MAT\>\""}], "]"}]}], ";",
     "\[IndentingNewLine]", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"Dimensions", "[", "IndexAllPre", "]"}], ";"}], "*)"}], 
    "\[IndentingNewLine]", 
    RowBox[{"IndexAll", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"First", "[", "IndexAllPre", "]"}], "]"}]}], ";", "\n", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"Dimensions", "[", "IndexAll", "]"}], ";"}], "*)"}], 
    "\[IndentingNewLine]", "\[IndentingNewLine]", 
    RowBox[{"HipPosIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "1"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"NSslopeIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "2"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"TorsoIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "3"}], "]"}], "]"}], "]"}]}], ";"}], 
   "*)"}], "\n", "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Lc", "\[Rule]", 
        FractionBox["3806", "10000"]}], ",", 
       RowBox[{"Lt", "\[Rule]", 
        FractionBox["4522", "10000"]}], ",", 
       RowBox[{"LT", "\[Rule]", 
        FractionBox["3273", "10000"]}], ",", 
       RowBox[{"mh", "\[Rule]", 
        FractionBox["471", "10"]}], ",", 
       RowBox[{"mt", "\[Rule]", 
        FractionBox["694", "100"]}], ",", 
       RowBox[{"mc", "\[Rule]", 
        FractionBox["323", "100"]}], ",", 
       RowBox[{"mf", "\[Rule]", 
        FractionBox["1006", "1000"]}], ",", 
       RowBox[{"g", "\[Rule]", 
        FractionBox["981", "100"]}]}], "}"}]}], ";"}], "*)"}], 
  RowBox[{"(*", 
   RowBox[{"mean", " ", "human", " ", "model"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Lc", "\[Rule]", 
        FractionBox["3806", "10000"]}], ",", 
       RowBox[{"Lt", "\[Rule]", 
        FractionBox["4522", "10000"]}], ",", 
       RowBox[{"LT", "\[Rule]", " ", "0"}], ",", 
       RowBox[{"mh", "\[Rule]", 
        FractionBox["471", "10"]}], ",", 
       RowBox[{"mt", "\[Rule]", 
        FractionBox["694", "100"]}], ",", 
       RowBox[{"mc", "\[Rule]", 
        FractionBox["323", "100"]}], ",", 
       RowBox[{"mf", "\[Rule]", 
        FractionBox["1006", "1000"]}], ",", 
       RowBox[{"g", "\[Rule]", 
        FractionBox["981", "100"]}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ndof", " ", "=", " ", "4"}], ";"}], " ", 
   RowBox[{"(*", "CHANGED", "*)"}], "\n", 
   RowBox[{
    RowBox[{"mm", "=", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "mf", ",", "mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc", ",", 
        "mf"}], "}"}], "/.", "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"statesubs", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", "i", "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "i"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", 
           RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"mm", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "mf", ",", "mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc", ",", 
         "mf"}], "}"}], "/.", "constsubs"}]}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"p0", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", " ", "0"}], ",", 
       
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", " ", "0"}], ",", 
       
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", " ", 
        "0"}], ",", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", " ", 
        "0"}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"q", "=", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"dq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ddq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"qe", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
       "q"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"dqe", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609372493992*^9}, {3.527609663926358*^9, 
   3.5276096847720747`*^9}, {3.527611654697356*^9, 3.527611678654079*^9}, {
   3.527618440974435*^9, 3.527618494722187*^9}, {3.52768382302354*^9, 
   3.527683839018791*^9}, {3.529267580216385*^9, 3.529267613766411*^9}, 
   3.529267683308161*^9, 3.52932321963372*^9, {3.52932496382654*^9, 
   3.52932497600504*^9}, {3.52932506748769*^9, 3.5293251607848997`*^9}, {
   3.529325287473012*^9, 3.529325341131288*^9}, {3.529325391332344*^9, 
   3.529325397891917*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
   "*)"}], 
  RowBox[{"(*", 
   RowBox[{"positive", " ", "z", " ", "direction"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "0"], "=", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "px"], "=", 
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "pz"], "=", 
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q1"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q2"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q3"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q4"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Xi]", "q5"], "=", 
      RowBox[{"RevoluteTwist", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}]}], ";"}], "*)"}], 
   RowBox[{"(*", "CHANGED", "*)"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"base", " ", "configuration"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{"1", "-", 
            RowBox[{"433", "/", "1000"}]}], ")"}], "Lc"}]}], "}"}]}], "]"}]}],
     ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"1", "-", 
             RowBox[{"433", "/", "1000"}]}], ")"}], "Lt"}]}]}], "}"}]}], 
      "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", "CHANGED", "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"1", "-", 
             RowBox[{"433", "/", "1000"}]}], ")"}], "Lt"}]}]}], "}"}]}], 
      "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "6"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{"1", "-", 
            RowBox[{"433", "/", "1000"}]}], ")"}], "Lc"}]}], "}"}]}], "]"}]}],
     ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "7"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "forward", " ", "kinematics", " ", "maps"}],
     "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   
   RowBox[{"(*", "CHANGED", "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "6"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "6"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   
   RowBox[{"(*", "CHANGED", "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "7"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "7"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   
   RowBox[{"(*", "CHANGED", "*)"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609372493992*^9}, {3.527609663926358*^9, 
   3.5276096847720747`*^9}, {3.527611654697356*^9, 3.527611678654079*^9}, {
   3.527618440974435*^9, 3.527618494722187*^9}, {3.52768382302354*^9, 
   3.527683839018791*^9}, {3.529267580216385*^9, 3.529267613766411*^9}, 
   3.529267683308161*^9, 3.52932321963372*^9, {3.52932496382654*^9, 
   3.52932497600504*^9}, {3.52932506748769*^9, 3.5293251607848997`*^9}, {
   3.529325287473012*^9, 3.529325341131288*^9}, {3.529325391332344*^9, 
   3.529325404672344*^9}, {3.529579895194458*^9, 3.5295798978822117`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
   RowBox[{"RPToHomogeneous", "[", 
    RowBox[{
     RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
   RowBox[{"RPToHomogeneous", "[", 
    RowBox[{
     RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}]}], ";"}], 
   "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
   RowBox[{"RPToHomogeneous", "[", 
    RowBox[{
     RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{"RPToHomogeneous", "[", 
    RowBox[{
     RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}],
    "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
  RowBox[{"TrigReduce", "[", 
   RowBox[{"ForwardKinematics", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Xi]", "q1"], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Xi]", "q2"], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Xi]", "q3"], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], 
   "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
  RowBox[{"Simplify", "[", 
   RowBox[{"ForwardKinematics", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Xi]", "q1"], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Xi]", "q2"], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Xi]", "q3"], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Xi]", "q4"], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609372493992*^9}, {3.527609663926358*^9, 
   3.5276096847720747`*^9}, {3.527611654697356*^9, 3.527611678654079*^9}, {
   3.527618440974435*^9, 3.527618494722187*^9}, {3.52768382302354*^9, 
   3.527683839018791*^9}, {3.529267580216385*^9, 3.529267613766411*^9}, 
   3.529267683308161*^9, 3.52932321963372*^9, {3.52932496382654*^9, 
   3.52932497600504*^9}, {3.52932506748769*^9, 3.5293251607848997`*^9}, {
   3.529325351027306*^9, 3.5293253668851023`*^9}, 3.5293254330324583`*^9, {
   3.529325475114431*^9, 3.5293255117665854`*^9}, 3.529325607494546*^9, {
   3.529325704434299*^9, 3.529325729441729*^9}, 3.529325772430676*^9, 
   3.5295797654368467`*^9, 3.5295798209959583`*^9, 3.529754159649246*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", "Lc"}], " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
       RowBox[{"Lt", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{"Lt", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}]}]},
     {"0", "1", "0", "0"},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}], 
      RowBox[{
       RowBox[{"Lc", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
       RowBox[{"Lt", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
       RowBox[{"Lt", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5295797666087723`*^9, 3.529579821422122*^9, 3.5295799318321543`*^9, 
   3.529583756975171*^9, 3.5297541465416*^9, 3.529765829339932*^9, 
   3.5297754136499*^9, {3.529780672319161*^9, 3.52978069220378*^9}, 
   3.529780766264474*^9, 3.530710678402088*^9, 3.530711229875464*^9, 
   3.5307112702993526`*^9, 3.530711414254718*^9, 3.5307187783760843`*^9, 
   3.530972138274044*^9, 3.5309775283508863`*^9, 3.530978115428659*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", "Lc"}], " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
       RowBox[{"Lt", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{"Lt", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{"Lc", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}]},
     {"0", "1", "0", "0"},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], 
      RowBox[{
       RowBox[{"Lc", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
       RowBox[{"Lt", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
       RowBox[{"Lt", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
       RowBox[{"Lc", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5295797666087723`*^9, 3.529579821422122*^9, 3.5295799318321543`*^9, 
   3.529583756975171*^9, 3.5297541465416*^9, 3.529765829339932*^9, 
   3.5297754136499*^9, {3.529780672319161*^9, 3.52978069220378*^9}, 
   3.529780766264474*^9, 3.530710678402088*^9, 3.530711229875464*^9, 
   3.5307112702993526`*^9, 3.530711414254718*^9, 3.5307187783760843`*^9, 
   3.530972138274044*^9, 3.5309775283508863`*^9, 3.530978115595677*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "aniplot", "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"pos", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
        "]"}], "/.", "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "center", " ", "of", " ", "mass"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            RowBox[{
             SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
            "\[LeftDoubleBracket]", 
            RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"2", "*", 
              RowBox[{"(", 
               RowBox[{"mf", "+", "mc", "+", "mt"}], ")"}]}], "+", "mh"}], 
            ")"}]}]}]}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], ";"}], 
   "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "kinetic", " ", "energy", " ", "and", " ", 
     "manipulator", " ", "inertia", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"For", "[", 
      RowBox[{
       RowBox[{"i", "=", "1"}], ",", 
       RowBox[{"i", "\[LessEqual]", 
        RowBox[{"Length", "[", "mm", "]"}]}], ",", 
       RowBox[{"i", "++"}], ",", 
       RowBox[{
        SubscriptBox["v", "i"], "=", 
        RowBox[{"Simplify", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[PartialD]", 
            RowBox[{"{", 
             RowBox[{"t", ",", "1"}], "}"}]], 
           RowBox[{"RigidPosition", "[", 
            RowBox[{
             SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], "]"}]}], "/.", 
          "\[InvisibleSpace]", "constsubs"}], "]"}]}]}], "]"}], ";", "\n", 
     RowBox[{"T", "=", 
      RowBox[{
       FractionBox["1", "2"], " ", 
       RowBox[{
        UnderoverscriptBox["\[Sum]", 
         RowBox[{"i", "=", "1"}], 
         RowBox[{"Length", "[", "mm", "]"}]], 
        RowBox[{
         RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}],
          " ", 
         RowBox[{
          SubscriptBox["v", "i"], ".", 
          SubscriptBox["v", "i"]}]}]}]}]}], ";", "\n", 
     RowBox[{"\[ScriptCapitalD]e", "=", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "dqe", "]"}], ",", "2"}], "}"}]], "T"}], "/.",
         "p0"}], "]"}]}], ";", "\n", 
     RowBox[{"(*", 
      RowBox[{
      "project", " ", "out", " ", "the", " ", "generalized", " ", 
       "coordinates", " ", "defining", " ", "the", " ", "position", " ", "of",
        " ", "the", " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ",
        "the", " ", "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], 
      "*)"}], "\[IndentingNewLine]", 
     RowBox[{"\[ScriptCapitalD]", "=", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"3", ";;", "All"}], ",", 
          RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
        "\[InvisibleSpace]", "p0"}], "]"}]}], ";", "\n", 
     RowBox[{"(*", 
      RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
     "\[IndentingNewLine]", 
     RowBox[{"\[ScriptCapitalC]", "=", 
      RowBox[{"Simplify", "[", 
       RowBox[{"InertiaToCoriolis", "[", 
        RowBox[{"\[ScriptCapitalD]", ",", 
         RowBox[{"Flatten", "[", "q", "]"}], ",", 
         RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{
      "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", 
       " ", "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\n", 
     RowBox[{"V", "=", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{"g", " ", 
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
           
           RowBox[{
            RowBox[{
             SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
            "\[LeftDoubleBracket]", 
            RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
        "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";", "\n", 
     RowBox[{"\[ScriptCapitalG]", "=", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
     "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"the", " ", "position", " ", "of", " ", "swing", " ", "foot"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{
      RowBox[{"Simplify", "[", 
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
         "\[RightDoubleBracket]"}]}], "]"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "\[Rule]", ".923"}]}], "}"}]}], ";"}], 
   "\n", 
   RowBox[{"h", "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "/.", "constsubs"}], "]"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"hdot", " ", "=", "  ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609372493992*^9}, {3.527609663926358*^9, 
   3.5276096847720747`*^9}, {3.527611654697356*^9, 3.527611678654079*^9}, {
   3.527618440974435*^9, 3.527618494722187*^9}, {3.52768382302354*^9, 
   3.527683839018791*^9}, {3.529267580216385*^9, 3.529267613766411*^9}, 
   3.529267683308161*^9, 3.52932321963372*^9, {3.52932496382654*^9, 
   3.52932497600504*^9}, {3.52932506748769*^9, 3.5293251607848997`*^9}, {
   3.529325351027306*^9, 3.5293253668851023`*^9}, 3.5293254330324583`*^9, 
   3.529325786929932*^9, {3.5293258513148737`*^9, 3.529325889828102*^9}, 
   3.529579775543784*^9, {3.530972101461122*^9, 3.5309721027148848`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"-", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", "Lc"}], " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
       RowBox[{"Lt", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{"Lt", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{"Lc", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{
        SubscriptBox["p", "x"], "(", "t", ")"}]}]},
     {"0", "1", "0", "0"},
     {
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], "0", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}], 
      RowBox[{
       RowBox[{"Lc", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
       RowBox[{"Lt", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
       RowBox[{"Lt", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
       RowBox[{"Lc", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{
        SubscriptBox["p", "z"], "(", "t", ")"}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529579776320292*^9, 3.529579932514986*^9, 3.5295837576158237`*^9, 
   3.529754169055911*^9, 3.529765830024757*^9, 3.529775417249385*^9, {
   3.529780673067926*^9, 3.529780692763152*^9}, 3.529780767011406*^9, 
   3.530710679013852*^9, 3.5307112304847307`*^9, 3.530711270929515*^9, 
   3.530711414864154*^9, 3.530718779024273*^9, 3.5309721473718767`*^9, 
   3.5309775333207607`*^9, 3.5309781160538387`*^9}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"1903", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
    RowBox[{"2261", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
    RowBox[{"2261", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
    RowBox[{"1903", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], "5000"], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529579776320292*^9, 3.529579932514986*^9, 3.5295837576158237`*^9, 
   3.529754169055911*^9, 3.529765830024757*^9, 3.529775417249385*^9, {
   3.529780673067926*^9, 3.529780692763152*^9}, 3.529780767011406*^9, 
   3.530710679013852*^9, 3.5307112304847307`*^9, 3.530711270929515*^9, 
   3.530711414864154*^9, 3.530718779024273*^9, 3.5309721473718767`*^9, 
   3.5309775333207607`*^9, 3.5309781162761097`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"testh", " ", "=", " ", 
  FractionBox[
   RowBox[{"(", 
    RowBox[{
     RowBox[{"1903", " ", 
      RowBox[{"Cos", "[", 
       SubscriptBox["x", "1"], "]"}]}], "+", 
     RowBox[{"2261", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        SubscriptBox["x", "1"], "+", 
        SubscriptBox["x", "2"]}], "]"}]}], "-", 
     RowBox[{"2261", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        SubscriptBox["x", "1"], "+", 
        SubscriptBox["x", "2"], "+", 
        SubscriptBox["x", "3"]}], "]"}]}], "-", 
     RowBox[{"1903", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        SubscriptBox["x", "1"], "+", 
        SubscriptBox["x", "2"], "+", 
        SubscriptBox["x", "3"], "-", 
        SubscriptBox["x", "4"]}], "]"}]}]}], ")"}], "5000"]}]], "Input",
 CellChangeTimes->{{3.5309742566058493`*^9, 3.530974392671496*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"1903", " ", 
     RowBox[{"cos", "(", 
      SubscriptBox["x", "1"], ")"}]}], "+", 
    RowBox[{"2261", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       SubscriptBox["x", "1"], "+", 
       SubscriptBox["x", "2"]}], ")"}]}], "-", 
    RowBox[{"2261", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       SubscriptBox["x", "1"], "+", 
       SubscriptBox["x", "2"], "+", 
       SubscriptBox["x", "3"]}], ")"}]}], "-", 
    RowBox[{"1903", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       SubscriptBox["x", "1"], "+", 
       SubscriptBox["x", "2"], "+", 
       SubscriptBox["x", "3"], "-", 
       SubscriptBox["x", "4"]}], ")"}]}]}], "5000"], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.530974325912006*^9, 3.53097434363369*^9}, 
   3.53097439337225*^9, 3.5309745567946978`*^9, 3.5309775432674503`*^9, 
   3.530978116419619*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"resulth", " ", "=", " ", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       FractionBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"1903", " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["x", "1"], "]"}]}], "+", 
          RowBox[{"2261", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["x", "1"], "+", 
             SubscriptBox["x", "2"]}], "]"}]}], "-", 
          RowBox[{"2261", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["x", "1"], "+", 
             SubscriptBox["x", "2"], "+", 
             SubscriptBox["x", "3"]}], "]"}]}], "-", 
          RowBox[{"1903", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["x", "1"], "+", 
             SubscriptBox["x", "2"], "+", 
             SubscriptBox["x", "3"], "-", 
             SubscriptBox["x", "4"]}], "]"}]}]}], ")"}], "5000"], "\[Equal]", 
       "0"}], ",", 
      SubscriptBox["x", "1"]}], "]"}], "//", "Simplify"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.530976679490596*^9, 3.5309767094979677`*^9}, {
   3.530976774394939*^9, 3.530976776629578*^9}, {3.5309769527502003`*^9, 
   3.530976989924376*^9}, {3.5309771864379253`*^9, 3.53097718942422*^9}, {
   3.5309772302598763`*^9, 3.530977281067944*^9}, {3.5309773181089363`*^9, 
   3.530977321059458*^9}, 3.530977552887101*^9}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"h1", " ", "=", " ", 
   RowBox[{"resulth", "[", 
    RowBox[{"[", 
     RowBox[{"1", ",", "1", ",", "2"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"h2", " ", "=", 
   RowBox[{"resulth", "[", 
    RowBox[{"[", 
     RowBox[{"2", ",", "1", ",", "2"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"h3", " ", "=", 
   RowBox[{"resulth", "[", 
    RowBox[{"[", 
     RowBox[{"3", ",", "1", ",", "2"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"h4", " ", "=", 
   RowBox[{"resulth", "[", 
    RowBox[{"[", 
     RowBox[{"4", ",", "1", ",", "2"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{" ", "h1", " "}]}], "Input",
 CellChangeTimes->{{3.53097794753203*^9, 3.530977993937746*^9}, 
   3.5309833190484877`*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   RowBox[{
    SuperscriptBox["cos", 
     RowBox[{"-", "1"}]], "(", 
    FractionBox[
     RowBox[{
      RowBox[{"2261", " ", 
       RowBox[{"sin", "(", 
        SubscriptBox["x", "2"], ")"}]}], "-", 
      RowBox[{"2261", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         SubscriptBox["x", "2"], "+", 
         SubscriptBox["x", "3"]}], ")"}]}], "-", 
      RowBox[{"1903", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         SubscriptBox["x", "2"], "+", 
         SubscriptBox["x", "3"], "-", 
         SubscriptBox["x", "4"]}], ")"}]}]}], 
     RowBox[{
      SqrtBox["2"], " ", 
      SqrtBox[
       RowBox[{
        RowBox[{"4302683", " ", 
         RowBox[{"cos", "(", 
          SubscriptBox["x", "2"], ")"}]}], "-", 
        RowBox[{"5112121", " ", 
         RowBox[{"cos", "(", 
          SubscriptBox["x", "3"], ")"}]}], "-", 
        RowBox[{"4302683", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["x", "2"], "+", 
           SubscriptBox["x", "3"]}], ")"}]}], "-", 
        RowBox[{"4302683", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["x", "3"], "-", 
           SubscriptBox["x", "4"]}], ")"}]}], "-", 
        RowBox[{"3621409", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["x", "2"], "+", 
           SubscriptBox["x", "3"], "-", 
           SubscriptBox["x", "4"]}], ")"}]}], "+", 
        RowBox[{"4302683", " ", 
         RowBox[{"cos", "(", 
          SubscriptBox["x", "4"], ")"}]}], "+", "8733530"}]]}]], ")"}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.530983319921051*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{"Solve", "[", 
   RowBox[{"h", "\[Equal]", "0"}], "]"}], "*)"}]], "Input",
 CellChangeTimes->{{3.530971177880711*^9, 3.530971188481676*^9}, {
  3.5309712241139297`*^9, 3.5309712271616697`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"torso", " ", "angle"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"theta4", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "thigh", " ", "angle"}]}], "*)"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"TorsoHipAngle", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], "*)"}], "*)"}], 
  "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"nstorso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
      RowBox[{"stance", " ", "Slope"}]}], "*)"}], "\n", 
    RowBox[{"LinearNStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
      RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], "\n", 
    RowBox[{"storso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";", "\n", 
    RowBox[{"(*", 
     RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], "*)"}], "\n", 
    RowBox[{"LinearStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"storso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{
    "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
    "*)"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"non", "-", 
    RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"nsslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
      "p0"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearNSslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"linearized", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"HipAngle", " ", "=", " ", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}], 
    RowBox[{"(*", 
     RowBox[{"hip", " ", "angle"}], "*)"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"HipPos", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"Hip", " ", "Position"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"LHipPos", " ", "=", " ", 
      RowBox[{
       RowBox[{
        RowBox[{"-", "Lc"}], " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
       RowBox[{"Lt", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}]}]}], 
     ";"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LHipPos", " ", "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{"HipPos", ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"Linearized", " ", "Hip", " ", "Position"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["xp", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt", "+", "mf"}], ")"}]}], "+", "mh"}], 
             ")"}]}]}]}], "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", 
      "Simplify"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["zp", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt", "+", "mf"}], ")"}]}], "+", "mh"}], 
             ")"}]}]}]}], "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", 
      "Simplify"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609372493992*^9}, {3.527609663926358*^9, 
   3.5276096847720747`*^9}, {3.527611654697356*^9, 3.527611678654079*^9}, {
   3.527618440974435*^9, 3.527618494722187*^9}, {3.52768382302354*^9, 
   3.527683839018791*^9}, {3.529267580216385*^9, 3.529267613766411*^9}, 
   3.529267683308161*^9, 3.52932321963372*^9, {3.52932496382654*^9, 
   3.52932497600504*^9}, {3.52932506748769*^9, 3.5293251607848997`*^9}, {
   3.529325351027306*^9, 3.5293253668851023`*^9}, 3.5293254330324583`*^9, 
   3.529325786929932*^9, {3.5293258513148737`*^9, 3.529325889828102*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"Pcom", " ", "=", " ", 
   SubscriptBox["xp", "COM"]}], ";"}], "\n", 
 RowBox[{"\[Delta]Pcom", "=", 
  RowBox[{"Linearize", "[", 
   RowBox[{
    RowBox[{"Pcom", "/.", "p0"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"Subscript", "[", 
        RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{"0", ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{"stcomslope", " ", "=", " ", 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      SubscriptBox["xp", "COM"]}], 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      SubscriptBox["zp", "COM"]}]], "/.", "p0"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"\[Delta]stcomslope", "=", 
   RowBox[{"Linearize", "[", 
    RowBox[{
     RowBox[{"stcomslope", "/.", "p0"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"Subscript", "[", 
         RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "4"}], "}"}]}], "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{"0", ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], "]"}]}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"\[Delta]stcomslopePre", "=", 
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"Normal", "[", 
       RowBox[{"Series", "@@", 
        RowBox[{"Prepend", "[", 
         RowBox[{
          RowBox[{"Table", "[", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], ",", "0", ",", 
              "1"}], "}"}], ",", 
            RowBox[{"{", 
             RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
          "stcomslope"}], "]"}]}], "]"}], "]"}]}], ";", "\n", 
    RowBox[{"\[Delta]stcomslope", "=", 
     RowBox[{
      UnderoverscriptBox["\[Sum]", 
       RowBox[{"i", "=", "1"}], "5"], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"\[Delta]stcomslopePre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "]"}], "/.", 
        "\[InvisibleSpace]", 
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{"#1", "\[Rule]", "0"}], "&"}], ")"}], "/@", 
         RowBox[{"Complement", "[", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", 
           RowBox[{"{", 
            RowBox[{
             SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}]}], 
          "]"}]}]}], ")"}]}]}], ";"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"nstcomslope", " ", "=", " ", 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      SubscriptBox["xp", "COM"]}], 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      SubscriptBox["zp", "COM"]}]], "/.", "p0"}]}], ";"}], "\n", 
 RowBox[{"\[Delta]nstcomslope", "=", 
  RowBox[{"Linearize", "[", 
   RowBox[{
    RowBox[{"nstcomslope", "/.", "p0"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"Subscript", "[", 
        RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{"0", ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], "]"}]}]}], "Input",
 CellChangeTimes->{{3.529323227844533*^9, 3.529323227992869*^9}, {
  3.529323267687533*^9, 3.529323287857778*^9}, {3.529330451050025*^9, 
  3.529330472803954*^9}, {3.529775560315535*^9, 3.5297756259267817`*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"-", 
     RowBox[{"24730236602", " ", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}]}], "-", 
    RowBox[{"11971114479", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "+", 
    RowBox[{"2094788099", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "-", 
    RowBox[{"457593477", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}]}], "34726000000"], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529406670628881*^9, 3.529579656026775*^9, 3.529579933002516*^9, 
   3.5295837581024857`*^9, 3.529754177900466*^9, 3.529765830589054*^9, {
   3.529775563909254*^9, 3.5297756262349157`*^9}, {3.5297806734856167`*^9, 
   3.529780693213313*^9}, 3.5297807676538467`*^9, 3.530710679504676*^9, 
   3.530711230921983*^9, 3.530711271407158*^9, 3.530711415310834*^9, 
   3.53071877955785*^9, 3.5309781177758703`*^9}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"-", 
     RowBox[{"24730236602", " ", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}]}], "-", 
    RowBox[{"11971114479", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "+", 
    RowBox[{"2094788099", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "-", 
    RowBox[{"457593477", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}]}], "24730236602"], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529406670628881*^9, 3.529579656026775*^9, 3.529579933002516*^9, 
   3.5295837581024857`*^9, 3.529754177900466*^9, 3.529765830589054*^9, {
   3.529775563909254*^9, 3.5297756262349157`*^9}, {3.5297806734856167`*^9, 
   3.529780693213313*^9}, 3.5297807676538467`*^9, 3.530710679504676*^9, 
   3.530711230921983*^9, 3.530711271407158*^9, 3.530711415310834*^9, 
   3.53071877955785*^9, 3.530978117800022*^9}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"-", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{"34726000000", " ", "Lc"}], "+", "11971114479"}], ")"}], " ", 
      
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}]}], "-", 
    RowBox[{"34726000000", " ", "Lc", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "+", 
    RowBox[{"34726000000", " ", "Lc", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], "-", 
    RowBox[{"34726000000", " ", "Lt", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "-", 
    RowBox[{"24730236602", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]}], "+", 
    RowBox[{"2094788099", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "-", 
    RowBox[{"457593477", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}]}], "24730236602"], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529406670628881*^9, 3.529579656026775*^9, 3.529579933002516*^9, 
   3.5295837581024857`*^9, 3.529754177900466*^9, 3.529765830589054*^9, {
   3.529775563909254*^9, 3.5297756262349157`*^9}, {3.5297806734856167`*^9, 
   3.529780693213313*^9}, 3.5297807676538467`*^9, 3.530710679504676*^9, 
   3.530711230921983*^9, 3.530711271407158*^9, 3.530711415310834*^9, 
   3.53071877955785*^9, 3.530978117829892*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["xp", "COM"], "//", "N"}], "//", 
  "Simplify"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["xp", "hip"], "//", "N"}], "//", "Simplify"}]}], "Input",
 CellChangeTimes->{{3.5307187930902233`*^9, 3.5307188186762123`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{"0.36742274154811955`", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
   RowBox[{"0.4050539243794275`", " ", 
    RowBox[{"sin", "(", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
   RowBox[{"0.047146075620572485`", " ", 
    RowBox[{"sin", "(", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
   RowBox[{"0.013177258451880435`", " ", 
    RowBox[{"sin", "(", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
      RowBox[{"1.`", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}]}], ")"}]}]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.5307187949803658`*^9, 3.530718819152948*^9}, 
   3.5309781179015417`*^9}],

Cell[BoxData[
 FormBox[
  SubscriptBox["xp", "hip"], TraditionalForm]], "Output",
 CellChangeTimes->{{3.5307187949803658`*^9, 3.530718819152948*^9}, 
   3.530978117903246*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
  "length", " ", "between", " ", "COM", " ", "and", " ", "stance", " ", 
   "foot"}], "*)"}]], "Input",
 CellChangeTimes->{{3.529326653272003*^9, 3.529326653783122*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{"stCOMLen", " ", "=", " ", 
   RowBox[{"h", "/", 
    RowBox[{"Sin", "[", 
     RowBox[{"ArcTan", "[", 
      RowBox[{
       FractionBox[
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["xp", "COM"]}], 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["zp", "COM"]}]], "/.", "p0"}], "]"}], "]"}]}]}], 
  "*)"}]], "Input",
 CellChangeTimes->{{3.529326673209874*^9, 3.5293267269492826`*^9}, {
  3.529326761678555*^9, 3.529326856916901*^9}, {3.529326952773411*^9, 
  3.529326955633334*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["xp", "hip"], "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "constsubs"}], 
   "/.", "p0"}]}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["zp", "hip"], "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "constsubs"}], 
   "/.", "p0"}]}]}], "Input",
 CellChangeTimes->{{3.529583484476513*^9, 3.529583530721816*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    FractionBox[
     RowBox[{"1903", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "5000"]}], 
   "-", 
   FractionBox[
    RowBox[{"2261", " ", 
     RowBox[{"sin", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "5000"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529583531516893*^9, 3.529583758180147*^9, 3.5297541805201893`*^9, 
   3.529765830688293*^9, {3.5297806737186613`*^9, 3.5297806933656073`*^9}, 
   3.529780767951338*^9, 3.530710679597867*^9, 3.5307112310738707`*^9, 
   3.530711271579677*^9, 3.530711415452495*^9, 3.530718779671402*^9, 
   3.530978117950642*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{
   FractionBox[
    RowBox[{"1903", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "5000"], "+", 
   
   FractionBox[
    RowBox[{"2261", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "5000"]}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529583531516893*^9, 3.529583758180147*^9, 3.5297541805201893`*^9, 
   3.529765830688293*^9, {3.5297806737186613`*^9, 3.5297806933656073`*^9}, 
   3.529780767951338*^9, 3.530710679597867*^9, 3.5307112310738707`*^9, 
   3.530711271579677*^9, 3.530711415452495*^9, 3.530718779671402*^9, 
   3.530978117955181*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["xp", "nsf"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "constsubs"}],
     "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["zp", "nsf"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
      "constsubs"}], "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"length", " ", "between", " ", "COM", " ", "and", " ", "non"}], 
    "-", 
    RowBox[{"stance", " ", "foot"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"nstCOMLen", " ", "=", " ", 
   RowBox[{"Sqrt", "[", 
    RowBox[{
     SuperscriptBox[
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["xp", "COM"], "-", 
        SubscriptBox["xp", "nsf"]}], ")"}], "2"], "+", 
     SuperscriptBox[
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["zp", "COM"], "-", 
        SubscriptBox["zp", "nsf"]}], ")"}], "2"]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"nstHipLen", " ", "=", " ", 
    RowBox[{"Sqrt", "[", 
     RowBox[{
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["xp", "hip"], "-", 
         SubscriptBox["xp", "nsf"]}], ")"}], "2"], "+", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["zp", "hip"], "-", 
         SubscriptBox["zp", "nsf"]}], ")"}], "2"]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"Linearized", " ", "stCOMLen"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{"\[Delta]nstCOMLen", "=", 
  RowBox[{"Linearize", "[", 
   RowBox[{
    RowBox[{"stCOMLen", "/.", "p0"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"Subscript", "[", 
        RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"Subscript", "[", 
        RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], 
   "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["xp", "stf"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "constsubs"}],
     "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["zp", "stf"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
      "constsubs"}], "/.", "p0"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
   "length", " ", "between", " ", "COM", " ", "and", " ", "stance", " ", 
    "foot"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stCOMLen", " ", "=", " ", 
   RowBox[{"Sqrt", "[", 
    RowBox[{
     SuperscriptBox[
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["xp", "COM"], "-", 
        SubscriptBox["xp", "stf"]}], ")"}], "2"], "+", 
     SuperscriptBox[
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["zp", "COM"], "-", 
        SubscriptBox["zp", "stf"]}], ")"}], "2"]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stHipLen", " ", "=", " ", 
   RowBox[{"Sqrt", "[", 
    RowBox[{
     SuperscriptBox[
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["xp", "hip"], "-", 
        SubscriptBox["xp", "stf"]}], ")"}], "2"], "+", 
     SuperscriptBox[
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["zp", "hip"], "-", 
        SubscriptBox["zp", "stf"]}], ")"}], "2"]}], "]"}]}], ";"}]}], "Input",\

 CellChangeTimes->{{3.5293259137171497`*^9, 3.529325966911764*^9}, {
   3.529326216593337*^9, 3.529326217432181*^9}, {3.529326575054517*^9, 
   3.5293266322949657`*^9}, {3.529326960111162*^9, 3.529326971619865*^9}, {
   3.529405999848874*^9, 3.529406003352455*^9}, {3.529579664062994*^9, 
   3.529579690984153*^9}, {3.52958346678754*^9, 3.529583511781439*^9}, {
   3.529583547818199*^9, 3.529583575221833*^9}, {3.529765822625334*^9, 
   3.529765851447948*^9}, {3.52976597298429*^9, 3.529765977363505*^9}, {
   3.5297756890339317`*^9, 3.5297757373553267`*^9}, 3.529777421714172*^9, {
   3.529777493456416*^9, 3.529777493850727*^9}, {3.530718954251514*^9, 
   3.530718964102365*^9}}],

Cell[BoxData[
 FormBox["stCOMLen", TraditionalForm]], "Output",
 CellChangeTimes->{3.530978118064783*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"nstCOMLen", "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}], "//", "N"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"nstHipLen", "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}], "//", "N"}]}], "Input",
 CellChangeTimes->{{3.5307188960340548`*^9, 3.530718912042986*^9}, {
  3.530718946254174*^9, 3.530719063290114*^9}}],

Cell[BoxData[
 FormBox["0.7121533318550942`", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5297774947803917`*^9, {3.5297806738107758`*^9, 3.529780693694281*^9}, 
   3.529780768051262*^9, 3.530710679646834*^9, 3.530711231371994*^9, 
   3.530711271971303*^9, 3.530711415763905*^9, 3.530718779732231*^9, 
   3.530718912650659*^9, {3.530719018409224*^9, 3.5307190637964573`*^9}, 
   3.530978118103508*^9}],

Cell[BoxData[
 FormBox["0.8328`", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5297774947803917`*^9, {3.5297806738107758`*^9, 3.529780693694281*^9}, 
   3.529780768051262*^9, 3.530710679646834*^9, 3.530711231371994*^9, 
   3.530711271971303*^9, 3.530711415763905*^9, 3.530718779732231*^9, 
   3.530718912650659*^9, {3.530719018409224*^9, 3.5307190637964573`*^9}, 
   3.5309781181047983`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["zp", "COM"], "*", 
  RowBox[{"Cos", "[", 
   RowBox[{"ArcTan", "[", "stcomslope", "]"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.529782868316927*^9, 3.529782933000318*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"12759122123", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
    RowBox[{"14065902578", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
    RowBox[{"1637194622", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
    RowBox[{"457593477", " ", 
     RowBox[{"cos", "(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
   RowBox[{"34726000000", " ", 
    SqrtBox[
     RowBox[{
      FractionBox[
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"12759122123", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"14065902578", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"1637194622", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"457593477", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}], "2"], 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"-", 
           RowBox[{"12759122123", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
          
          RowBox[{"14065902578", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"1637194622", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"457593477", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}], "2"]], "+", "1"}]]}]], TraditionalForm]], "Output",
 CellChangeTimes->{{3.529782871184145*^9, 3.529782933571473*^9}, 
   3.5307106797091722`*^9, 3.53071123144198*^9, 3.530711272062455*^9, 
   3.5307114158264923`*^9, 3.530718779818125*^9, 3.530978118170586*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 SuperscriptBox[
  RowBox[{"(", "stCOMLen", ")"}], "2"]], "Input",
 CellChangeTimes->{{3.529782418385331*^9, 3.5297824309690638`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   FractionBox[
    SuperscriptBox[
     RowBox[{"(", 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"12759122123", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
       RowBox[{"14065902578", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{"1637194622", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
       RowBox[{"457593477", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], ")"}],
      "2"], "1205895076000000000000"], "+", 
   FractionBox[
    SuperscriptBox[
     RowBox[{"(", 
      RowBox[{
       RowBox[{"12759122123", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
       RowBox[{"14065902578", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
       RowBox[{"1637194622", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
       RowBox[{"457593477", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], ")"}],
      "2"], "1205895076000000000000"]}], TraditionalForm]], "Output",
 CellChangeTimes->{{3.529780673879572*^9, 3.5297806937809067`*^9}, 
   3.52978076821041*^9, 3.52978243187782*^9, 3.530710679768182*^9, 
   3.530711231520227*^9, 3.530711272214872*^9, 3.530711415905674*^9, 
   3.5307187798974524`*^9, 3.530978118208849*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{"subs", "=", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"x", "[", 
        RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", 
       RowBox[{"x0", "[", 
        RowBox[{"[", "i", "]"}], "]"}]}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], ";"}], "*)"}]], "Input",
 CellChangeTimes->{{3.529781212581707*^9, 3.529781212583466*^9}, 
   3.530711286562608*^9, 3.530711345472354*^9, {3.530711398226095*^9, 
   3.530711401871519*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{"f0", "=", 
    RowBox[{"f", "/.", "subs"}]}], ";", "\n", 
   RowBox[{"Jf0", "=", 
    RowBox[{
     RowBox[{"D", "[", 
      RowBox[{"f", ",", 
       RowBox[{"{", "x", "}"}]}], "]"}], "/.", "subs"}]}], ";", "\n", 
   RowBox[{"dx", "=", 
    RowBox[{"Transpose", "[", 
     RowBox[{"{", 
      RowBox[{"x", "-", "x0"}], "}"}], "]"}]}], ";", "\n", 
   RowBox[{
    RowBox[{"Flatten", "[", 
     RowBox[{"f0", "+", 
      RowBox[{"Jf0", ".", "dx"}]}], "]"}], "//", "Simplify"}]}], 
  "*)"}]], "Input",
 CellChangeTimes->{{3.529781212581707*^9, 3.529781212583466*^9}, 
   3.530711286562608*^9, {3.530711407464031*^9, 3.530711410158873*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Table", "[", 
  RowBox[{
   RowBox[{
    RowBox[{"Subscript", "[", 
     RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
   RowBox[{"{", 
    RowBox[{"i", ",", "4"}], "}"}]}], "]"}]], "Input",
 CellChangeTimes->{{3.529781052644586*^9, 3.529781055856134*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ",", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], ",", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], ",", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], "}"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.529781056303488*^9, 3.53071068006257*^9, 
  3.53071123166089*^9, 3.5307112723300323`*^9, 3.5307114159878397`*^9, 
  3.530718779914568*^9, 3.530978118252503*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Tmat", " ", "=", " ", 
  RowBox[{
   RowBox[{"D", "[", 
    RowBox[{"stCOMLen", ",", 
     RowBox[{"{", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "4"}], "}"}]}], "]"}], "}"}]}], "]"}], "/.", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}]}]], "Input",
 CellChangeTimes->{{3.5297774730188837`*^9, 3.5297774760993137`*^9}, {
  3.529781002411879*^9, 3.529781002860035*^9}, {3.529781067225656*^9, 
  3.5297810848005657`*^9}, {3.529781560998413*^9, 3.529781609965416*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{"0", ",", "0", ",", "0", ",", "0"}], "}"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.529781561533641*^9, 3.529781610753256*^9, 
  3.530710680090732*^9, 3.5307112317120047`*^9, 3.5307112724173393`*^9, 
  3.530711416042069*^9, 3.530718779988346*^9, 3.530978118265737*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Dimensions", "[", "Tmat", "]"}]], "Input",
 CellChangeTimes->{{3.529781072346712*^9, 3.529781075867608*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", "4", "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{3.530710680121592*^9, 3.530711231767352*^9, 
  3.530711272610448*^9, 3.530711416055739*^9, 3.530718780000469*^9, 
  3.53097811832708*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData["Tsub"], "Input",
 CellChangeTimes->{{3.529781146962158*^9, 3.529781150498324*^9}}],

Cell[BoxData[
 FormBox["Tsub", TraditionalForm]], "Output",
 CellChangeTimes->{3.529781151197033*^9, 3.5307106801265287`*^9, 
  3.530711231824727*^9, 3.5307112727029333`*^9, 3.530711416064682*^9, 
  3.530718780005527*^9, 3.530978118436267*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Tsub", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"Subscript", "[", 
     RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], "\[Rule]", 
   "0"}]}]], "Input",
 CellChangeTimes->{{3.529781142317834*^9, 3.52978119332591*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "i"], "(", "t", ")"}], "\[Rule]", "0"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.529781174203965*^9, 3.529781193979351*^9}, 
   3.5307106801317987`*^9, 3.530711231831409*^9, 3.530711272713491*^9, 
   3.530711416073944*^9, 3.5307187800096207`*^9, 3.530978118445612*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"Linearized", " ", "stCOMLen"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"\[Delta]stCOMLen", "=", 
   RowBox[{"Linearize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["zp", "COM"], "*", 
       RowBox[{"Cos", "[", 
        RowBox[{"ArcTan", "[", "stcomslope", "]"}], "]"}]}], "/.", "p0"}], 
     ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"Subscript", "[", 
         RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "4"}], "}"}]}], "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{"0", ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], "]"}]}]}]], "Input",
 CellChangeTimes->{{3.5293259137171497`*^9, 3.529325966911764*^9}, {
   3.529326216593337*^9, 3.529326217432181*^9}, {3.529326575054517*^9, 
   3.5293266322949657`*^9}, {3.529326960111162*^9, 3.529326971619865*^9}, {
   3.529405999848874*^9, 3.529406003352455*^9}, {3.529579664062994*^9, 
   3.529579690984153*^9}, {3.52958346678754*^9, 3.529583511781439*^9}, {
   3.529583547818199*^9, 3.529583575221833*^9}, {3.529765822625334*^9, 
   3.529765851447948*^9}, {3.52976597298429*^9, 3.529765977363505*^9}, {
   3.5297756890339317`*^9, 3.5297757373553267`*^9}, 3.529777421714172*^9, {
   3.529780659261392*^9, 3.529780664693989*^9}, {3.529781113725379*^9, 
   3.529781113855076*^9}, {3.529781251911324*^9, 3.529781256192069*^9}, {
   3.529781325123919*^9, 3.529781370364891*^9}, {3.529782440041555*^9, 
   3.529782444999276*^9}, 3.529782951466254*^9}],

Cell[BoxData[
 FormBox[
  FractionBox["12365118301", "17363000000"], TraditionalForm]], "Output",
 CellChangeTimes->{{3.5295796646561813`*^9, 3.5295796913536177`*^9}, 
   3.5295799331204147`*^9, 3.529583758302537*^9, 3.529754183013183*^9, {
   3.529765830785973*^9, 3.529765851890555*^9}, {3.529765973362669*^9, 
   3.529765977753004*^9}, {3.529775691180711*^9, 3.529775724785117*^9}, {
   3.529780660940892*^9, 3.529780693935842*^9}, 3.52978076857415*^9, 
   3.529781114448728*^9, {3.529781249482316*^9, 3.529781256631266*^9}, {
   3.52978132778231*^9, 3.529781370683909*^9}, {3.529782440825265*^9, 
   3.5297824454083014`*^9}, 3.529782952288382*^9, 3.5307106801570663`*^9, 
   3.530711231844492*^9, 3.530711272779471*^9, 3.530711416148052*^9, 
   3.5307187800642138`*^9, 3.53097811848213*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[Delta]stCOMLen", "=", 
  RowBox[{"Linearize", "[", 
   RowBox[{
    RowBox[{"stCOMLen", "/.", "p0"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"Subscript", "[", 
        RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{"0.000001", ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], "]"}]}]], "Input",
 CellChangeTimes->{{3.5297813847847424`*^9, 3.529781384940415*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{"1.5353339419690806`*^-7", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}]}], "+", 
   RowBox[{"8.226235203947351`*^-8", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "-", 
   RowBox[{"7.670932937561792`*^-9", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], "+", 
   "0.7121533318551335`"}], TraditionalForm]], "Output",
 CellChangeTimes->{3.529781385310704*^9, 3.530710680194821*^9, 
  3.5307112318954697`*^9, 3.5307112728628283`*^9, 3.530711416218202*^9, 
  3.5307187801200953`*^9, 3.5309781185363483`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"COM", " ", "Angle"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"COMAngle", " ", "=", " ", 
   RowBox[{
    RowBox[{"ArcTan", "[", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["xp", "COM"]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["zp", "COM"]}]], "/.", "p0"}], "]"}], " ", "-", 
    RowBox[{"ArcTan", "[", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["xp", "COM"]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["zp", "COM"]}]], "/.", "p0"}], "]"}]}]}]}]], "Input",
 CellChangeTimes->{{3.529753849438113*^9, 3.529753854349491*^9}, {
  3.529754076827833*^9, 3.5297541231978283`*^9}, {3.529754204619493*^9, 
  3.529754213479525*^9}, {3.529758955813388*^9, 3.529758962039299*^9}, {
  3.5297590092458076`*^9, 3.529759015072678*^9}, {3.530710435515167*^9, 
  3.530710458558079*^9}, {3.530710588523739*^9, 3.53071062019357*^9}, {
  3.530711207548511*^9, 3.5307112228379917`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{
    SuperscriptBox["tan", 
     RowBox[{"-", "1"}]], "(", 
    FractionBox[
     RowBox[{
      RowBox[{"12759122123", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
      RowBox[{"14065902578", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
      RowBox[{"1637194622", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
      RowBox[{"457593477", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
     RowBox[{
      RowBox[{"-", 
       RowBox[{"12759122123", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
      RowBox[{"14065902578", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
      RowBox[{"1637194622", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
      RowBox[{"457593477", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}]], ")"}],
    "-", 
   RowBox[{
    SuperscriptBox["tan", 
     RowBox[{"-", "1"}]], "(", 
    FractionBox[
     RowBox[{
      RowBox[{"-", 
       RowBox[{"Lc", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "+", 
      RowBox[{"Lc", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
      RowBox[{"Lt", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
      RowBox[{"Lt", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
      FractionBox[
       RowBox[{
        RowBox[{"12759122123", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"14065902578", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"1637194622", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"457593477", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
       "34726000000"]}], 
     RowBox[{
      RowBox[{"Lc", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
      RowBox[{"Lc", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
      RowBox[{"Lt", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
      RowBox[{"Lt", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
      FractionBox[
       RowBox[{
        RowBox[{"-", 
         RowBox[{"12759122123", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
        RowBox[{"14065902578", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"1637194622", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"457593477", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
       "34726000000"]}]], ")"}]}], TraditionalForm]], "Output",
 CellChangeTimes->{
  3.529754124127902*^9, {3.529754186958247*^9, 3.529754214073298*^9}, 
   3.529765831050655*^9, 3.529775636773246*^9, {3.529780674014739*^9, 
   3.529780694065446*^9}, 3.5297807687400713`*^9, 3.53071068026175*^9, 
   3.53071123201857*^9, 3.530711272978052*^9, 3.5307114163552437`*^9, 
   3.530718780245062*^9, 3.53097811866575*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[Delta]COMAngle", "=", 
  RowBox[{"Linearize", "[", 
   RowBox[{
    RowBox[{"COMAngle", "/.", "p0"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{"Subscript", "[", 
        RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{"0", ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "4"}], "}"}]}], "]"}]}], "]"}]}]], "Input",
 CellChangeTimes->{{3.529775652066016*^9, 3.529775670260892*^9}}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{"17363000000", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{"Lc", "+", "Lt"}], ")"}], " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], "+", 
      RowBox[{"Lc", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], "-", 
      RowBox[{"Lc", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}]}], ")"}]}], 
   "12365118301"], TraditionalForm]], "Output",
 CellChangeTimes->{{3.5297756533893433`*^9, 3.529775670856402*^9}, {
   3.5297806741406193`*^9, 3.52978069414636*^9}, 3.529780768870967*^9, 
   3.5307106803171587`*^9, 3.530711232085804*^9, 3.530711273083034*^9, 
   3.530711416434854*^9, 3.530718780311173*^9, 3.530978118797744*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"Hip", " ", "Angle"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"HipAngle", " ", "=", " ", 
   RowBox[{
    RowBox[{"ArcTan", "[", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["xp", "hip"]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["zp", "hip"]}]], "/.", "p0"}], "]"}], " ", "-", 
    RowBox[{"ArcTan", "[", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["xp", "hip"]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        SubscriptBox["zp", "hip"]}]], "/.", "p0"}], "]"}]}]}]}]], "Input",
 CellChangeTimes->{{3.530710415618435*^9, 3.530710421786216*^9}, {
  3.530710504229053*^9, 3.530710515697269*^9}, {3.530710559871928*^9, 
  3.530710607028942*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   RowBox[{
    SuperscriptBox["tan", 
     RowBox[{"-", "1"}]], "(", 
    FractionBox[
     RowBox[{
      FractionBox[
       RowBox[{"1903", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "5000"], 
      "+", 
      FractionBox[
       RowBox[{"2261", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], 
       "5000"]}], 
     RowBox[{
      RowBox[{"-", 
       FractionBox[
        RowBox[{"1903", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "5000"]}],
       "-", 
      FractionBox[
       RowBox[{"2261", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], 
       "5000"]}]], ")"}], "-", 
   RowBox[{
    SuperscriptBox["tan", 
     RowBox[{"-", "1"}]], "(", 
    FractionBox[
     RowBox[{
      RowBox[{"-", 
       RowBox[{"Lc", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "+", 
      RowBox[{"Lc", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
      RowBox[{"Lt", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
      RowBox[{"Lt", " ", 
       RowBox[{"sin", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
      FractionBox[
       RowBox[{"1903", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "5000"], 
      "+", 
      FractionBox[
       RowBox[{"2261", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], 
       "5000"]}], 
     RowBox[{
      RowBox[{"Lc", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
      RowBox[{"Lc", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
      RowBox[{"Lt", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
      RowBox[{"Lt", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
      FractionBox[
       RowBox[{"1903", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "5000"], 
      "-", 
      FractionBox[
       RowBox[{"2261", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], 
       "5000"]}]], ")"}]}], TraditionalForm]], "Output",
 CellChangeTimes->{3.530710680445837*^9, 3.530711232191662*^9, 
  3.5307112731824703`*^9, 3.5307114165354443`*^9, 3.530718780409439*^9, 
  3.530978118932013*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"EuclideanDistance", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["xp", "COM"], ",", 
       SubscriptBox["zp", "COM"]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["xp", "nsf"], ",", 
       SubscriptBox["zp", "nsf"]}], "}"}]}], "]"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"\[Delta]nstcomslopePre", "=", 
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"Normal", "[", 
       RowBox[{"Series", "@@", 
        RowBox[{"Prepend", "[", 
         RowBox[{
          RowBox[{"Table", "[", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], ",", "0", ",", 
              "1"}], "}"}], ",", 
            RowBox[{"{", 
             RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
          "nstcomslope"}], " ", "]"}]}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"\[Delta]nstcomslope", "=", 
     RowBox[{
      UnderoverscriptBox["\[Sum]", 
       RowBox[{"i", "=", "1"}], "5"], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"\[Delta]nstcomslopePre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "]"}], "/.", 
        "\[InvisibleSpace]", 
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{"#1", "\[Rule]", "0"}], "&"}], ")"}], "/@", 
         RowBox[{"Complement", "[", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", 
           RowBox[{"{", 
            RowBox[{
             SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}]}], 
          "]"}]}]}], ")"}]}]}], ";"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{"-", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}], ";"}], 
   "*)"}], 
  RowBox[{"(*", 
   RowBox[{"Dimensionless", " ", "Linearized", " ", "Hip", " ", "Position"}], 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"feedback", " ", "control"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"\[Chi]", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"q", ",", "dq"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"d\[Chi]", "=", 
     RowBox[{"D", "[", 
      RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"Hip", " ", "Position", " ", 
       RowBox[{"Output", ":", 
        RowBox[{"1.", " ", "hip", " ", "position"}]}]}], ";", 
      RowBox[{
      "2.", " ", "Linearized", " ", "hip", " ", "position", " ", "3.", " ", 
       "Dimensionless", " ", "Linearized", " ", "hip", " ", "position"}]}], 
     "*)"}], "\n", 
    RowBox[{"hipOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"HipPos", ",", "LHipPos"}], "}"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["p", "hip"], " ", "=", 
     RowBox[{"hipOutput", "[", 
      RowBox[{"[", "HipPosIndex", "]"}], "]"}]}], " ", ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["p", "hipdot"], "  ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["v", "hip"], "=", 
     RowBox[{
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
    ";", "\n", 
    RowBox[{"fb", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         SubscriptBox["p", "hip"], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"time", "-", 
      RowBox[{"invariant", " ", 
       RowBox[{"parameterization", ":"}]}]}], "*)"}], "\n", 
    RowBox[{"\[Sigma]y", "=", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
       RowBox[{"Exp", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "4"}], "]"}], 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "5"}], "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"calculate", 
      FormBox[
       SubscriptBox["y", "d"],
       TraditionalForm], "and", " ", "its", " ", "derivatives"}], "*)"}], 
    "\n", "\n", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], " ", "=", " ", 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
      "Simplify"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}]}], ";", "\n", 
    RowBox[{"(*", 
     RowBox[{
     "calculate", " ", "actual", " ", "kinematics", " ", "outputs", " ", 
      "and", " ", "Jacobians"}], "*)"}], "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["p", "hip"], ",", "t"}], "]"}]}], ";", "\n", 
    RowBox[{"NSslopeOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], "}"}]}], 
    ";", "\[IndentingNewLine]", " ", "\[IndentingNewLine]", 
    RowBox[{"TorsoOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
      "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
       ",", "storso", ",", "LinearStorso"}], "}"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"NSslopeOutput", "[", 
            RowBox[{"[", "NSslopeIndex", "]"}], "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"TorsoOutput", " ", "[", 
            RowBox[{"[", "TorsoIndex", "]"}], "]"}], "}"}]}], "}"}], "/.", 
        "constsubs"}], "/.", "p0"}], "]"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "*)"}], "\n", "\n", 
  RowBox[{"(*", 
   RowBox[{"zero", " ", "dynamics"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Zstatesubs", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"z", ",", " ", "1"}], "]"}], "[", "t", "]"}], " ", "->", 
        " ", 
        FormBox[
         RowBox[{"z", "[", "1", "]"}],
         TraditionalForm]}], ",", " ", "\n", "  ", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"z", ",", " ", "2"}], "]"}], "[", "t", "]"}], " ", "->", 
        " ", 
        FormBox[
         RowBox[{"z", "[", "2", "]"}],
         TraditionalForm]}]}], "}"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"c", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"Flatten", "[", 
         SubscriptBox["p", "hip"], "]"}]}], " ", "/.", " ", "constsubs"}], 
      "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"H", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"Flatten", "[", 
         SubscriptBox["y", 
          RowBox[{"a", ",", "2"}]], "]"}]}], " ", "/.", " ", "constsubs"}], 
      "]"}]}], ";", "\n", 
    RowBox[{"Phi", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", "c", "}"}], ",", "H"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"fbz", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
            RowBox[{"p", "[", "1", "]"}]}], ")"}], "/", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}]}], "}"}], "\n", 
       RowBox[{"Subscript", "[", 
        RowBox[{"yz", ",", "d", ",", "2"}], "]"}]}], "=", 
      RowBox[{
       RowBox[{"Transpose", "[", 
        RowBox[{"{", 
         RowBox[{"Table", "[", 
          RowBox[{
           RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
           RowBox[{"{", 
            RowBox[{"i", ",", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
        "]"}], "/.", 
       RowBox[{"fbz", "\n", 
        RowBox[{"PhiI", "=", 
         RowBox[{"Simplify", "[", 
          RowBox[{
           RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
           RowBox[{"Join", "[", 
            RowBox[{
             RowBox[{"{", 
              RowBox[{"{", 
               RowBox[{
                RowBox[{"Subscript", "[", 
                 RowBox[{"z", ",", "1"}], "]"}], "[", "t", "]"}], "}"}], 
              "}"}], ",", 
             RowBox[{"Subscript", "[", 
              RowBox[{"yz", ",", "d", ",", "2"}], "]"}]}], "]"}]}], 
          "]"}]}]}]}]}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"PhidotI", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Subscript", "[", 
             RowBox[{"z", ",", "2"}], "]"}], "[", "t", "]"}], "}"}], "}"}], 
         ",", 
         RowBox[{
          RowBox[{"D", "[", 
           RowBox[{
            RowBox[{"Subscript", "[", 
             RowBox[{"yz", ",", "d", ",", "2"}], "]"}], ",", 
            RowBox[{
             RowBox[{"Subscript", "[", 
              RowBox[{"z", ",", "1"}], "]"}], "[", "t", "]"}]}], "]"}], "*", 
          RowBox[{
           RowBox[{"Subscript", "[", 
            RowBox[{"z", ",", "2"}], "]"}], "[", "t", "]"}]}]}], "]"}]}], 
      "]"}]}], ";"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
   "write", " ", "the", " ", "control", " ", "files", " ", "to", " ", 
    "disk"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_ntorso\>\""}],
      "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<stCOMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"stcomslope", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltastCOMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"\[Delta]stcomslope", " ", "/.", "p0"}], "/.", 
        "\[InvisibleSpace]", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<nstCOMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"nstcomslope", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltanstCOMFn\>\"", "]"}]}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"\[Delta]nstcomslope", " ", "/.", "p0"}], "/.", 
        "\[InvisibleSpace]", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<hipposFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"HipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaHipposFn\>\"", "]"}]}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"LHipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<nsslopeFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"nsslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaNSslopeFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"LinearNSslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]",
         "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<COMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"Pcom", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaCOMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"\[Delta]Pcom", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<stCOMLenFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"stCOMLen", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<nstCOMLenFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"nstCOMLen", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<stHipLenFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"stHipLen", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<nstHipLenFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"nstHipLen", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<COMAngleFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"COMAngle", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<HipAngleFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"HipAngle", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h1Fn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"h1", "/.", "p0"}], "/.", "\[InvisibleSpace]", "constsubs"}], 
       "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h2Fn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"h2", "/.", "p0"}], "/.", "\[InvisibleSpace]", "constsubs"}], 
       "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h3Fn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"h3", "/.", "p0"}], "/.", "\[InvisibleSpace]", "constsubs"}], 
       "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h4Fn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"h4", "/.", "p0"}], "/.", "\[InvisibleSpace]", "constsubs"}], 
       "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"stream", "=", 
      RowBox[{"OpenWrite", "[", "\"\<stCOMLenFn\>\"", "]"}]}], ";", "\n", 
     RowBox[{"Write", "[", 
      RowBox[{"stream", ",", 
       RowBox[{
        RowBox[{
         RowBox[{"Pcom", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
         "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
     RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
     RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
     RowBox[{"stream", "=", 
      RowBox[{"OpenWrite", "[", "\"\<nstCOMLenFn\>\"", "]"}]}], ";", "\n", 
     RowBox[{"Write", "[", 
      RowBox[{"stream", ",", 
       RowBox[{
        RowBox[{
         RowBox[{"\[Delta]Pcom", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]",
          "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
     RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
     RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609372493992*^9}, {3.527609663926358*^9, 
   3.5276096847720747`*^9}, {3.527611654697356*^9, 3.527611678654079*^9}, {
   3.527618440974435*^9, 3.527618494722187*^9}, {3.52768382302354*^9, 
   3.527683839018791*^9}, {3.529267580216385*^9, 3.529267613766411*^9}, 
   3.529267683308161*^9, 3.52932321963372*^9, {3.529323330284863*^9, 
   3.529323350988159*^9}, {3.529323561639594*^9, 3.529323588307955*^9}, {
   3.529324823400998*^9, 3.5293248890194817`*^9}, {3.529325977978808*^9, 
   3.529326096740509*^9}, {3.529326129298633*^9, 3.529326192074296*^9}, {
   3.5293262592363567`*^9, 3.529326311859496*^9}, {3.52932700196033*^9, 
   3.529327020339204*^9}, {3.529327332577093*^9, 3.529327334947711*^9}, {
   3.52940661868822*^9, 3.529406663737935*^9}, {3.529583700425022*^9, 
   3.529583720888556*^9}, {3.5297542245401773`*^9, 3.52975424449712*^9}, {
   3.5307106420926313`*^9, 3.530710650649394*^9}, {3.530978008802866*^9, 
   3.5309780492800694`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";", 
  RowBox[{"Run", "[", "\"\<perl math2mat_output.pl\>\"", "]"}]}]], "Input",
 CellChangeTimes->{{3.527609439405788*^9, 3.5276094613465033`*^9}, 
   3.527683848666622*^9}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5276095314524193`*^9, 3.527609724746904*^9, 3.527611733948635*^9, 
   3.5276185215193243`*^9, 3.527683862407487*^9, 3.529267699044849*^9, 
   3.5292677443265247`*^9, 3.529267784996078*^9, 3.529323390249442*^9, 
   3.5293235930528803`*^9, 3.529327342359889*^9, 3.52940667111942*^9, 
   3.529406707620516*^9, 3.52957965629456*^9, 3.529579933422922*^9, 
   3.5295800343628683`*^9, 3.529583758618491*^9, {3.529754331217762*^9, 
   3.5297543486476383`*^9}, 3.5297544064741077`*^9, 3.529765831168281*^9, {
   3.529780674363693*^9, 3.529780694255344*^9}, 3.52978076914688*^9, 
   3.530710680701128*^9, 3.5307112323177443`*^9, 3.530711273301057*^9, 
   3.530711416627022*^9, 3.5307187807395153`*^9, 3.530978119208404*^9, 
   3.530978215269781*^9, 3.5309783217457247`*^9, 3.5309783880657177`*^9, 
   3.5309787796153297`*^9, {3.530979001842156*^9, 3.530979017177085*^9}, 
   3.5309794557009087`*^9, 3.530979495793459*^9, 3.530987227808565*^9, 
   3.530987326780726*^9, 3.5309883509284763`*^9, 3.530989170556691*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"FolderName", " ", "=", " ", 
     RowBox[{"\"\<build_torso_\>\"", "<>", 
      RowBox[{"ToString", "[", "LineNumber", "]"}]}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
    ";"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Run", "[", "\"\<perl math2mat_torso_1.pl\>\"", "]"}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"Run", "[", 
     RowBox[{"\"\<mkdir \>\"", "<>", "FolderName"}], "]"}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"Run", "[", 
     RowBox[{
     "\"\<cp ./build_torso/*.m ./\>\"", "<>", " ", "FolderName", "<>", 
      "\"\</\>\""}], "]"}], ";"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs/build_torso\>\"", 
     "]"}], ";"}], "*)"}], "\[IndentingNewLine]"}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527609009585134*^9, 
   3.52760906708628*^9}, {3.527609142962778*^9, 3.527609278637491*^9}, {
   3.5276093131512003`*^9, 3.527609379763487*^9}, {3.527609413085588*^9, 
   3.5276094249617023`*^9}}]
},
WindowSize->{1920, 1026},
WindowMargins->{{0, Automatic}, {Automatic, 0}},
ShowSelection->True,
Magnification:>FEPrivate`If[
  FEPrivate`Equal[FEPrivate`$VersionNumber, 6.], 1.25, 1.25 Inherited],
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[545, 20, 3782, 76, 244, "Input"],
Cell[4330, 98, 8231, 218, 622, "Input"],
Cell[12564, 318, 15732, 449, 673, "Input"],
Cell[CellGroupData[{
Cell[28321, 771, 8032, 218, 296, "Input"],
Cell[36356, 991, 3744, 101, 119, "Output"],
Cell[40103, 1094, 4908, 131, 119, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[45048, 1230, 12706, 328, 604, "Input"],
Cell[57757, 1560, 5033, 135, 119, "Output"],
Cell[62793, 1697, 1589, 42, 82, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[64419, 1744, 869, 26, 71, "Input"],
Cell[65291, 1772, 915, 28, 82, "Output"]
}, Open  ]],
Cell[66221, 1803, 1444, 37, 71, "Input"],
Cell[CellGroupData[{
Cell[67690, 1844, 842, 27, 129, "Input"],
Cell[68535, 1873, 1676, 52, 91, "Output"]
}, Open  ]],
Cell[70226, 1928, 233, 5, 36, "Input"],
Cell[70462, 1935, 11319, 300, 631, "Input"],
Cell[CellGroupData[{
Cell[81806, 2239, 4422, 129, 289, "Input"],
Cell[86231, 2370, 980, 24, 82, "Output"],
Cell[87214, 2396, 978, 24, 82, "Output"],
Cell[88195, 2422, 1440, 37, 82, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[89672, 2464, 290, 8, 62, "Input"],
Cell[89965, 2474, 1319, 38, 53, "Output"],
Cell[91287, 2514, 175, 4, 59, "Output"]
}, Open  ]],
Cell[91477, 2521, 210, 5, 36, "Input"],
Cell[91690, 2528, 913, 25, 59, "Input"],
Cell[CellGroupData[{
Cell[92628, 2557, 675, 21, 64, "Input"],
Cell[93306, 2580, 846, 24, 82, "Output"],
Cell[94155, 2606, 822, 23, 82, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[95014, 2634, 4865, 147, 315, "Input"],
Cell[99882, 2783, 105, 2, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[100024, 2790, 1252, 42, 60, "Input"],
Cell[101279, 2834, 409, 7, 53, "Output"],
Cell[101691, 2843, 399, 7, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[102127, 2855, 213, 5, 36, "Input"],
Cell[102343, 2862, 4296, 117, 132, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[106676, 2984, 148, 3, 39, "Input"],
Cell[106827, 2989, 2960, 79, 147, "Output"]
}, Open  ]],
Cell[109802, 3071, 533, 15, 36, "Input"],
Cell[110338, 3088, 690, 20, 106, "Input"],
Cell[CellGroupData[{
Cell[111053, 3112, 296, 8, 36, "Input"],
Cell[111352, 3122, 556, 15, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[111945, 3142, 1111, 33, 36, "Input"],
Cell[113059, 3177, 332, 7, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[113428, 3189, 132, 2, 36, "Input"],
Cell[113563, 3193, 236, 5, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[113836, 3203, 96, 1, 36, "Input"],
Cell[113935, 3206, 243, 4, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[114215, 3215, 250, 7, 36, "Input"],
Cell[114468, 3224, 358, 8, 53, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[114863, 3237, 1616, 35, 60, "Input"],
Cell[116482, 3274, 794, 12, 82, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[117313, 3291, 582, 16, 36, "Input"],
Cell[117898, 3309, 653, 16, 55, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[118588, 3330, 1769, 45, 100, "Input"],
Cell[120360, 3377, 7881, 216, 185, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[128278, 3598, 573, 16, 36, "Input"],
Cell[128854, 3616, 815, 21, 82, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[129706, 3642, 1525, 42, 105, "Input"],
Cell[131234, 3686, 4674, 141, 122, "Output"]
}, Open  ]],
Cell[135923, 3830, 28801, 794, 3143, "Input"],
Cell[CellGroupData[{
Cell[164749, 4628, 282, 6, 36, "Input"],
Cell[165034, 4636, 1086, 16, 53, "Output"]
}, Open  ]],
Cell[166135, 4655, 2544, 50, 175, "Input"]
}
]
*)

(* End of internal cache information *)
